/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "calc_cor_eng_adapt.h"
#include <assert.h>


/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcCorEngAdapt
* FUNCTION
*               Calculate Correlation and Energy
* SYNOPSIS
*               CalcCorEngAdapt(AnaSignal, MatchSignal, length,
*                       correlation, energy)
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       AnaSignal       fxpt_16  i      Signal to analyze
*       MatchSignal     fxpt_16  i      Signal to match
*       length          int      i      Length of signals
*       correlation     fxpt_32  o      Correlation between to signals
*       energy          fxpt_32  o      Energy in signal to analyze
*
*
**************************************************************************/

void CalcCorEngAdapt(
fxpt_32 AnaSignal[],                    /* 18.13 format */ /* used to be 15.0 format! */
fxpt_16 MatchSignal[],                  /* 15.0 format */
fxpt_32 *correlation,                   /* 30.1 format */
fxpt_32 *energy)                        /* 30.1 format */
{
  int           i;
  int64         ener_acc;
  int64         corr_acc;
  fxpt_16	*pm;
  fxpt_32	*pa;
  fxpt_32 m32;
  /* 32 bits may be inadequate to hold the non-normalized
   * correlation and energy values.  Since we are interested
   * in the relative values anyway, we scale these values
   * by shifting them to the right 4 times (* 1/16).
   */

  ener_acc = 0;
  corr_acc = 0;
/*
  for(i=0; i<SF_LEN; i++) {
    ener_acc += (int64)AnaSignal[i] * AnaSignal[i];
    corr_acc += (int64)AnaSignal[i] * MatchSignal[i];
  }
*/
  for(i=SF_LEN, pa=AnaSignal, pm=MatchSignal ; i ; --i, ++pa, ++pm) {
	ener_acc += (int64)*pa * *pa;

	m32= *pm;
	corr_acc += (int64)*pa * m32;
  }

//  *energy = ener_acc >> 3;
//  *correlation = corr_acc >> 3;
  *energy = fxpt_saturate32_round( ener_acc, 29 );
  *correlation = fxpt_saturate32_round( corr_acc, 16 );
}

